#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "Types.h"
#include "Data.h"
#include "ViterbiLib.h"

VOID
ViterbiRemoveNoise(
		FLOAT* OutputVector,
		UINT8* DemodulatedVector,
		INT32  BitsNo
		)
{
	UINT16 idx = 0;
	/* Decodes buffer received from channel */
	for(idx = 0 ; idx < BitsNo ; idx++)
	{
		if(OutputVector[idx] < 0)
		{
			DemodulatedVector[idx] = 0;
		}
		else
		{
			DemodulatedVector[idx] = 1;
		}
	}
}

UINT8
ViterbiCompareDuoBits(
		VITERBI_STATES*	StatesTable,
		UINT8*			DemodulatedVector,
		UINT16			Row,
		UINT32			Column,
		UINT8			OutBinValue
		)
{
	UINT8 Distance = 0;
	UINT8 idx 	   = 0;

	switch(OutBinValue)
	{
		case 0:
			for(idx = 0 ; idx < 2 ; idx++)
			{
				if(StatesTable[Row].Info[0].Output[idx] != DemodulatedVector[idx])
				{
					Distance++;
				}
			}
			break;
		case 1:
			for(idx = 0 ; idx < 2 ; idx++)
			{
				if(StatesTable[Row].Info[1].Output[idx] != DemodulatedVector[idx])
				{
					Distance++;
				}
			}
			break;
		default:
			return 0;
	}
	return Distance;
}

VOID
ViterbiCalcMetric(
		POINT		    (*Point)[MAX_FRAME],
		VITERBI_STATES*	StatesTable,
		UINT8*			DemodulatedVector,
		UINT16			MaxRows,
		UINT32			MaxColumns
		)
{
	UINT16	Row	   = 0;
	UINT32	Column = 0;
	UINT16	DuoBit = 0;

	for(Column = 0 ; Column < MaxColumns - 1 ; Column++)
	{
		for(Row = 0 ; Row < MaxRows ; Row++)
		{
			if(Point[Row][Column].Active == TRUE)
			{
				Point[Row][Column].Distance.HmgZero = ViterbiCompareDuoBits(StatesTable, &DemodulatedVector[DuoBit], Row, Column, 0);
				Point[Row][Column].Distance.HmgOne  = ViterbiCompareDuoBits(StatesTable, &DemodulatedVector[DuoBit], Row, Column, 1);
#if 0
				printf("%d%d\n ", Point[Row][Column].Distance.HmgZero, Point[Row][Column].Distance.HmgOne);
#endif
			}
		}
		DuoBit+=2;
	}
}

VOID
ViterbiCalcMinCumulDist(
				POINT  (*Point)[MAX_FRAME],
				UINT16	MaxRows,
				UINT32	MaxColumns
				)
{
	UINT32	Column = 0;
	UINT16	Row	   = 0;
	UINT16  CalculatedCumulDist = 0;

	for(Column = 0 ; Column < MaxColumns ; Column++)
	{
		for(Row = 0 ; Row < MaxRows ; Row++)
		{
				Point[Row][Column].MinCumulDist = CalculatedCumulDist;
		}
		CalculatedCumulDist += 2;
	}
}

VOID
ViterbiFillCrateGraph(
		POINT		    (*Point)[MAX_FRAME],
		VITERBI_STATES*	StatesTable,
		UINT16			MaxRows,
		UINT32			MaxColumns
		)
{
	UINT32	Column = 0;
	UINT16	Row	   = 0;

	Point[0][0].Active = TRUE;

	for(Column = 0 ; Column < MaxColumns - 1 ; Column++)
	{
		for(Row = 0 ; Row < MaxRows ; Row++)
		{
			if(Point[Row][Column].Active == TRUE)
			{
				Point[Row][Column].Row	  = Row;
				Point[Row][Column].Column = Column;
				
				Point[Row][Column].NextStateZero		 = &Point[StatesTable[Row].Info[0].NextState][Column+1];
				Point[Row][Column].NextStateOne			 = &Point[StatesTable[Row].Info[1].NextState][Column+1];
				Point[Row][Column].NextStateZero->Active = TRUE;
				Point[Row][Column].NextStateOne->Active  = TRUE;
			}
		}
	}

	ViterbiCalcMinCumulDist(Point,
							MaxRows,
							MaxColumns
							);
}

VOID
ViterbiCalcOutput(
		UINT8*	Register,
		UINT8*	Output,
		UINT8	BinValue
		)
{
	Output[0] = ( ((*(Register) & BIT_0)  >> 0) +
		    	  ((*(Register) & BIT_1)  >> 1) +
		    	  ((*(Register) & BIT_2)  >> 2) +
		    	  ((*(Register) & BIT_3)  >> 3) +
		    	  ((*(Register) & BIT_6)  >> 6) ) % 2;

	Output[1] = ( ((*(Register) & BIT_0)  >> 0) +
			      ((*(Register) & BIT_2)  >> 2) +
			      ((*(Register) & BIT_3)  >> 3) +
			      ((*(Register) & BIT_5)  >> 5) +
			      ((*(Register) & BIT_6)  >> 6) ) % 2;
}

VOID
ViterbiFillStateTable(
		VITERBI_STATES* StatesTable, 
		UINT8			Row
		)
{
	UINT16	idx = 0;

	StatesTable->State = Row;
		
	StatesTable->Info[0].Input = 0;
	StatesTable->Info[1].Input = 1;

	StatesTable->Info[0].Register = StatesTable->State;

	StatesTable->Info[1].Register = StatesTable->State;
	StatesTable->Info[1].Register += BIT_6;

	StatesTable->Info[0].NextState = ((StatesTable->Info[0].Register & STATE) >> 1);
	StatesTable->Info[1].NextState = ((StatesTable->Info[1].Register & STATE) >> 1);

	for(idx = 0 ; idx < ARRAY_SIZE(StatesTable->Info) ; idx++)
	{
		ViterbiCalcOutput(
				&StatesTable->Info[idx].Register,
				StatesTable->Info[idx].Output,
				idx
				);
	}
}

VOID
ViterbiDecideWhenAllStates(
		POINT		(*Point)[MAX_FRAME],
		UINT16		Row,
		UINT32		Column
		)
{
	if( (Point[Row][Column].CumulDist + Point[Row][Column].Distance.HmgZero) <=
		 Point[Row][Column].NextStateZero->MinCumulDist)
	{
		Point[Row][Column].NextStateZero->MinCumulDist = Point[Row][Column].CumulDist +
														 Point[Row][Column].Distance.HmgZero;

		Point[Row][Column].NextStateZero->CumulDist    = Point[Row][Column].NextStateZero->MinCumulDist;

		memcpy(Point[Row][Column].NextStateZero->Sequence,
			   Point[Row][Column].Sequence, Column);
		Point[Row][Column].NextStateZero->Sequence[Column] = 0;
	}

	if( (Point[Row][Column].CumulDist + Point[Row][Column].Distance.HmgOne) <=
		 Point[Row][Column].NextStateOne->MinCumulDist)
	{
		Point[Row][Column].NextStateOne->MinCumulDist = Point[Row][Column].CumulDist +
														Point[Row][Column].Distance.HmgOne;
		Point[Row][Column].NextStateOne->CumulDist	  = Point[Row][Column].NextStateOne->MinCumulDist;

		memcpy(Point[Row][Column].NextStateOne->Sequence,
			   Point[Row][Column].Sequence, Column);
		Point[Row][Column].NextStateOne->Sequence[Column] = 1;
	}
}

VOID
ViterbiDecideNotAllStates(
		POINT		(*Point)[MAX_FRAME],
		UINT16		Row,
		UINT32		Column
		)
{
	Point[Row][Column].NextStateZero->CumulDist = Point[Row][Column].CumulDist +
												  Point[Row][Column].Distance.HmgZero;

	memcpy(Point[Row][Column].NextStateZero->Sequence,
		   Point[Row][Column].Sequence, Column);
	Point[Row][Column].NextStateZero->Sequence[Column] = 0;

	Point[Row][Column].NextStateOne->CumulDist  = Point[Row][Column].CumulDist +
												  Point[Row][Column].Distance.HmgOne;

	memcpy(Point[Row][Column].NextStateOne->Sequence,
		   Point[Row][Column].Sequence, Column);
	Point[Row][Column].NextStateOne->Sequence[Column] = 1;
}

VOID
ViterbiCopyToDecodedVector(
		POINT		(*Point)[MAX_FRAME],
		UINT8*		DecodedVector,
		UINT16		MaxRows,
		UINT32		MaxColumns)
{
	UINT8	MemoryCumulDist = 0;
	UINT8	MemoryState 	= 0;

	MemoryCumulDist = Point[0][MaxColumns-1].CumulDist;

	for(UINT8 Row = 0 ; Row < MaxRows ; Row++)
	{
		if(Point[Row][MaxColumns-1].CumulDist < MemoryCumulDist)
		{
			MemoryCumulDist = Point[Row][MaxColumns-1].CumulDist;
			MemoryState		= Row;
		}
	}
#if 0
	printf("\n%d. BestCumulDist: %d\n", MemoryState, Point[MemoryState][MaxColumns-1].CumulDist);
	for(idx = 0 ; idx < MaxColumns - 1 ; idx++)
		printf("%d",  Point[MemoryState][MaxColumns - 1].Sequence[idx]);
#endif
	memcpy(DecodedVector, Point[MemoryState][MaxColumns - 1].Sequence, MaxColumns - 1);
}

VOID
ViterbiMakeHardDecision(
		POINT		(*Point)[MAX_FRAME],
		UINT8*		DecodedVector,
		UINT16		MaxRows,
		UINT32		MaxColumns
		)
{
	UINT16	Row    = 0;
	UINT32	Column = 0;

	for(Column = 0 ; Column < MaxColumns ; Column++ )
	{
		for(Row = 0 ; Row < MaxRows ; Row++)
		{
			if(Column < ALL_STATES_FILLED &&
			   Point[Row][Column].Active == TRUE)
			{

				ViterbiDecideNotAllStates(Point,
										  Row,
										  Column);
			}
			if(Column >= ALL_STATES_FILLED &&
			   Point[Row][Column].Active == TRUE &&
			   Column < MaxColumns - 1)
			{
				ViterbiDecideWhenAllStates(Point,
										   Row,
										   Column);
			}
		}
	}
	ViterbiCopyToDecodedVector(Point,
							   DecodedVector,
							   MaxRows,
							   MaxColumns);
#if 0	/* Print sequences for all metrics */
	for(UINT8 Row = 0 ; Row < MaxRows ; Row++)
	{
		printf("\n%d. CumulDist: %d\n", Row, Point[Row][MaxColumns-1].CumulDist);
		for(int i = 0 ; i < MaxColumns - 1 ; i++)
			printf("%d",  Point[Row][MaxColumns - 1].Sequence[i]);
	}
#endif
}


UINT8
ViterbiDecoder(
		FLOAT* ReceivedFrame,
		UINT8* DecodedVector,
		INT32  BitsNo
		)
{
	static VITERBI_STATES StatesTable[STATES_COUNT];
	static POINT		  Point[STATES_COUNT][MAX_FRAME];

	UINT8		  DemodulatedVector[MAX_FRAME] = {0};
	UINT16		  Row 					   	   =  0;
	UINT32		  PointsPerRowCount			   = (BitsNo / 2) + 1;

	ViterbiRemoveNoise(ReceivedFrame, DemodulatedVector, BitsNo);

	for(Row = 0; Row < ARRAY_SIZE(StatesTable) ; Row++)
	{
		ViterbiFillStateTable(&StatesTable[Row], Row);
	}

	ViterbiFillCrateGraph(Point,
					  StatesTable,
					  STATES_COUNT,
					  PointsPerRowCount
					  );

	ViterbiCalcMetric(Point,
					  StatesTable,
					  DemodulatedVector,
					  STATES_COUNT,
					  PointsPerRowCount
					  );

	ViterbiMakeHardDecision(Point,
					  DecodedVector,
					  STATES_COUNT,
					  PointsPerRowCount
				  	  );
	return 1;
}
